package physics;

import java.lang.*;
import java.util.*;
import entity.GameObject;
import world.*;

// This is a very basic rigid body dynamics physics package with collision detection.
public class Physics {
	
	// This contains all game objects subject to physics.
	private Vector<GameObject> physicsBodies;
	
	public Vector<GameObject> getPhysicsBodies() {
		return physicsBodies;
	}

	public void setPhysicsBodies(Vector<GameObject> physicsBodies) {
		this.physicsBodies = physicsBodies;
	}

	// This is the gravity constant for the physics world to be implemented in m/s^2.
	private float g;
	
	// This is the threshold to zero out velocities to prevent bodies from never going
	// to rest in m/s.
	private float dampThreshold = 0.1f;
	
	
	// This is a link to the chunk world in order to determine the min/max coordinates.
	private World currentWorld= new World();
	private int chunkSize = Chunk.getBlocksPerCubicChunk();
	private int worldSize[] =currentWorld.getWorldSize();; 
	private int totalXBlocks = chunkSize * worldSize[0];
	private int totalYBlocks = chunkSize * worldSize[1];
	private int totalZBlocks = chunkSize * worldSize[2];
	
	// Constructor - initializes the physicsBodies vector and sets gravity to Earth,
	// default of -9.81 m/s^2.  Creates a default world for sizing.
	public Physics(){
		physicsBodies = new Vector<GameObject>();
		g = -9.81f;
		currentWorld = new World();
		worldSize =currentWorld.getWorldSize();
	}

	// Constructor - initializes the physicsBodies vector and sets gravity to the parameter.
	// Links the game world to the physics engine.
	public Physics(float gravity, World world){
		physicsBodies = new Vector<GameObject>();
		g = gravity;
		currentWorld = world;
	}

	// Update function - runs the world for dt milliseconds and applies physics to
	// all objects in physicsBodies.
	public void update(float dt){
		Iterator i = physicsBodies.iterator();
		Iterator j = physicsBodies.iterator();
		GameObject currentBody, collisionBody;
		float currentBodyminX, currentBodyminY, currentBodyminZ;
		float currentBodymaxX, currentBodymaxY, currentBodymaxZ;
		float collisionBodyminX = 0, collisionBodyminY = 0, collisionBodyminZ = 0;
		float collisionBodymaxX = currentWorld.getWorldSize()[0], collisionBodymaxY = currentWorld.getWorldSize()[1], collisionBodymaxZ = currentWorld.getWorldSize()[2];
		float currentXPos, currentYPos, currentZPos;
		float currentXVel, currentYVel, currentZVel;
		float currentXForce, currentYForce, currentZForce;
		float nextXPos, nextYPos, nextZPos;
		float testx, testy, testz;
		float velMod;
		boolean notCollided = true;
		boolean xCollision = false;
		boolean yCollision = false;
		boolean zCollision = false;
		while (i.hasNext()){
			currentBody = (GameObject) i.next();
			
			//If the body is not static then process it.
			if (!currentBody.isStatic){
				currentXPos = currentBody.getPosX();
				currentYPos = currentBody.getPosY();
				currentZPos = currentBody.getPosZ();
				currentXForce = currentBody.getForceX();
				currentYForce = currentBody.getForceY();
				currentZForce = currentBody.getForceZ();
				currentXVel = currentBody.getVelX();
				currentYVel = currentBody.getVelY();
				currentZVel = currentBody.getVelZ();
				//If not grounded, include gravity (F=ma)
				if(!currentBody.isGrounded){
					currentYForce = currentYForce + (g*currentBody.getMass());
					currentYVel = (currentYVel + (currentYForce / currentBody.getMass() * dt)) * currentBody.getFriction();
				}
				else {
					currentYVel = 0;
				}
				// Apply forces to get updated velocity: F/m = a, v1 = v0 + at
				currentXVel = (currentXVel + ((currentXForce+currentBody.getFriction()) / currentBody.getMass() * dt));
				currentZVel = (currentZVel + ((currentZForce+currentBody.getFriction()) / currentBody.getMass() * dt));

				// If velocity is below damping threshold, zero out velocity.
				if (currentXVel < dampThreshold) {
					currentXVel = 0;
				}
				if (currentYVel < dampThreshold) {
					currentYVel = 0;
				}
				if (currentZVel < dampThreshold) {
					currentZVel = 0;
				}
				
				currentBody.setVel(currentXVel, currentYVel, currentZVel);
				// Use current velocity to determine next position
				nextXPos = currentXPos + (currentXVel * dt);
				nextYPos = currentYPos + (currentYVel * dt);
				nextZPos = currentZPos + (currentZVel * dt);
				// Find out if we collide with terrain
				// Account for height/radius based on the direction the object is moving
				if (currentXVel<0) {
					testx = currentBody.getPosX() - currentBody.getRadius();
				}
				else {
					testx = currentBody.getPosX() + currentBody.getRadius();
				}
				// Y axis is the vertical
				if (currentYVel<0) {
					testy = currentBody.getPosY() - currentBody.getHeight();
				}
				else {
					testx = currentBody.getPosY();
				}
				
				if (currentZVel<0) {
					testz = currentBody.getPosZ() - currentBody.getRadius();
				}
				else {
					testz = currentBody.getPosZ() + currentBody.getRadius();
				}
				
				testy = currentBody.getPosY();
				testz = currentBody.getPosZ();
				notCollided = true;

				// Check x axis for collision
				while (testx != nextXPos && notCollided ){
					
					try {
						
						if (currentWorld.getManager().emptyChunkCheck(currentWorld.getManager().getChunk((int)(testx ), (int)(testy ), (int)(testz ))))
						{
							if (currentXVel<0) {
								testx = testx - 1;
							}
							else{
								testx = testx + 1;
							}
						}
						else {
							notCollided = false;
							if (currentXVel<0) {
								testx = testx + 1;
							}
							else{
								testx = testx - 1;
							}

						}
					} catch (Exception e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
						System.exit(-1);
					}
				}

				// Set nextXPos to testx: if there was a collision then testx will not make it all the way to nextXPos and will be corrected back 1 block to account for that
				// Otherwise testx will reach nextXPos and the assignment is merely redundant.  This will also achieve a slide along the wall with other axes of movement
				currentBody.setNextX(testx);
				currentBody.setVelX(testx-currentXPos);
				
				// Check z axis for collision
				notCollided = true;
				while (testz != nextZPos && notCollided){
					if (currentWorld.getManager().emptyChunkCheck(currentWorld.getManager().getChunk((int)(testx * chunkSize), (int)(testy * chunkSize), (int)(testz * chunkSize)))){
						if (currentZVel<0) {
							testz = testz - 1;
						}
						else{
							testz = testz + 1;
						}
					}
					else {
						notCollided = false;
						if (currentZVel<0) {
							testz = testz + 1;
						}
						else{
							testz = testz - 1;
						}

					}
				}
				currentBody.setNextX(testx);
				currentBody.setVelZ(testz-currentZPos);
				
				// Check y axis (vertical) for collision
				notCollided = true;
				while (testy != nextYPos && notCollided){
					if(currentWorld.getManager().emptyChunkCheck(currentWorld.getManager().getChunk((int)(testx * chunkSize), (int)(testy * chunkSize), (int)(testz * chunkSize)))){
						if (currentYVel<0) {
							testy = testy - 1;
						}
						else{
							testy = testy + 1;
						}
					}
					else {
						notCollided = false;
						// If there is a collision with a downward y velocity, the object will now be grounded.
						if (currentYVel<0) {
							testy = testy + 1;
							currentBody.isGrounded = true;
						}
						else{
							testy = testy - 1;
						}

					}
				}
				currentBody.setNextY(testy);
				currentBody.setVelY(testy-currentYPos);			
				
			} // if currentBody is not static
		} // end while
		
		// Check for collisions against other entities
		i = physicsBodies.iterator();
		while (i.hasNext()){
			currentBody = (GameObject) i.next();
			if (!currentBody.isStatic){
				// Check to see if currentPos plus current vel will put us in collision with anything, if so, reduce the velocity in the axis that the collision occurs
				// Build the bounding cube corners for the currentBody
				if (currentBody.getVelX()<0){
					currentBodyminX = currentBody.getPosX() + (currentBody.getVelX()*dt) - currentBody.getRadius();
					currentBodymaxX = currentBody.getPosX() + currentBody.getRadius();
				}
				else {
					currentBodyminX = currentBody.getPosX() - currentBody.getRadius();
					currentBodymaxX = currentBody.getPosX() + (currentBody.getVelX()*dt) - currentBody.getRadius();
				}
				if (currentBody.getVelY()<0){
					currentBodyminY = currentBody.getPosY() + (currentBody.getVelY()*dt);
					currentBodymaxY = currentBody.getPosY() + currentBody.getHeight();
				}
				else {
					currentBodyminY = currentBody.getPosY();
					currentBodymaxY = currentBody.getPosY() + currentBody.getHeight() + (currentBody.getVelY()*dt);
				}
				if (currentBody.getVelZ()<0){
					currentBodyminZ = currentBody.getPosZ() + (currentBody.getVelZ()*dt) - currentBody.getRadius();
					currentBodymaxZ = currentBody.getPosZ() + currentBody.getRadius();
				}
				else {
					currentBodyminZ = currentBody.getPosZ() - currentBody.getRadius();
					currentBodymaxZ = currentBody.getPosZ() + (currentBody.getVelZ()*dt) - currentBody.getRadius();
				}
				j = physicsBodies.iterator();
				while (j.hasNext()){
					xCollision = false;
					yCollision = false;
					zCollision = false;
					collisionBody = (GameObject) j.next();
					if (!(currentBody==collisionBody)){
						// Build the bounding cube corners for the collisionBody to be checked against
						if (collisionBody.getVelX()<0){
							currentBodyminX = collisionBody.getPosX() + (collisionBody.getVelX()*dt) - collisionBody.getRadius();
							currentBodymaxX = collisionBody.getPosX() + collisionBody.getRadius();
						}
						else {
							currentBodyminX = collisionBody.getPosX() - collisionBody.getRadius();
							currentBodymaxX = collisionBody.getPosX() + (collisionBody.getVelX()*dt) - collisionBody.getRadius();
						}
						if (collisionBody.getVelY()<0){
							currentBodyminY = collisionBody.getPosY() + (collisionBody.getVelY()*dt);
							currentBodymaxY = collisionBody.getPosY() + collisionBody.getHeight();
						}
						else {
							currentBodyminY = collisionBody.getPosY();
							currentBodymaxY = collisionBody.getPosY() + collisionBody.getHeight() + (collisionBody.getVelY()*dt);
						}
						if (collisionBody.getVelZ()<0){
							currentBodyminZ = collisionBody.getPosZ() + (collisionBody.getVelZ()*dt) - collisionBody.getRadius();
							currentBodymaxZ = collisionBody.getPosZ() + collisionBody.getRadius();
						}
						else {
							currentBodyminZ = collisionBody.getPosZ() - collisionBody.getRadius();
							currentBodymaxZ = collisionBody.getPosZ() + (collisionBody.getVelZ()*dt) - collisionBody.getRadius();
						}
						
						// Separating axis theorem on AABB on each axis in turn
						if (((currentBodyminX > collisionBodyminX) && (currentBodyminX < collisionBodymaxX)) || ((currentBodymaxX > collisionBodyminX) && (currentBodymaxX < collisionBodymaxX))){
							xCollision = true;
						}
						if (((currentBodyminY > collisionBodyminY) && (currentBodyminY < collisionBodymaxY)) || ((currentBodymaxY > collisionBodyminY) && (currentBodymaxY < collisionBodymaxY))){
							yCollision = true;
						}
						if (((currentBodyminZ > collisionBodyminZ) && (currentBodyminZ < collisionBodymaxZ)) || ((currentBodymaxZ > collisionBodyminZ) && (currentBodymaxZ < collisionBodymaxZ))){
							zCollision = true;
						}
						
						// If collision on all three axes, collision between the two shapes.
						if (xCollision && yCollision && zCollision){
							if ((currentBodyminX > collisionBodyminX) && (currentBodyminX < collisionBodymaxX)){
								velMod = (collisionBodymaxX - currentBodyminX) / (2*dt);
								currentBody.setVelX(currentBody.getVelX()+velMod);
								collisionBody.setVelX(collisionBody.getVelX()-velMod);
							}
							else {
								velMod = (currentBodymaxX - collisionBodyminX) / (2*dt);
								currentBody.setVelX(currentBody.getVelX()-velMod);
								collisionBody.setVelX(collisionBody.getVelX()+velMod);								
							}
							if ((currentBodyminY > collisionBodyminY) && (currentBodyminY < collisionBodymaxY)){
								velMod = (collisionBodymaxY - currentBodyminY) / (2*dt);
								currentBody.setVelY(currentBody.getVelY()+velMod);
								collisionBody.setVelY(collisionBody.getVelY()-velMod);								
							}
							else {
								velMod = (currentBodymaxY - collisionBodyminY) / (2*dt);
								currentBody.setVelY(currentBody.getVelY()-velMod);
								collisionBody.setVelY(collisionBody.getVelY()+velMod);																
							} 
							if ((currentBodyminZ > collisionBodyminZ) && (currentBodyminZ < collisionBodymaxZ)){
								velMod = (collisionBodymaxZ - currentBodyminZ) / (2*dt);
								currentBody.setVelZ(currentBody.getVelZ()+velMod);
								collisionBody.setVelZ(collisionBody.getVelZ()-velMod);								
							}
							else{
								velMod = (currentBodymaxZ - collisionBodyminZ) / (2*dt);
								currentBody.setVelZ(currentBody.getVelZ()-velMod);
								collisionBody.setVelZ(collisionBody.getVelZ()+velMod);								
							}
						}
					}

				}
			}
			
		}
		
		// Final position updates
		i = physicsBodies.iterator();
		while (i.hasNext()){
			currentBody = (GameObject) i.next();
			if (!currentBody.isStatic){
				currentBody.setPosX(currentBody.getPosX()+(currentBody.getVelX()*dt));
				currentBody.setPosY(currentBody.getPosY()+(currentBody.getVelY()*dt));
				currentBody.setPosZ(currentBody.getPosZ()+(currentBody.getVelZ()*dt));

				if ((currentBody.getVelX()==0)&&(currentBody.getVelY()==0)&&(currentBody.getVelZ()==0)){
					currentBody.isStatic = true;
				}
			}
		}
	}
	
	// addBody - adds the GameObject to the physicsBodies vector for physics processing.
	// Also adds the body to its space partition
	public void addBody(GameObject newObject){
		physicsBodies.add(newObject);		
	}
	
	// removeBody - removes the GameObject from the physicsBodies vector.  Nothing happens
	// if the body is not found.
	public void removeBody(GameObject oldObject){
		physicsBodies.remove(oldObject);
	}

	public World getCurrentWorld() {
		return currentWorld;
	}

	public void setCurrentWorld(World currentWorld) {
		this.currentWorld = currentWorld;
	}
}
